作動関節マニピュレータの逆運動学

Created by Masahiro Furukawa, Aug 18, 2020

image.png

In [1]:
#参考URL -> https://qiita.com/tibigame/items/61cecf86fc978628bfee
#参考図書 -> ポールのロボット・マニピュレータ

import numpy as np
import sympy as sym
sym.init_printing()
Pi = sym.S.Pi # 円周率
E = sym.S.Exp1 # 自然対数の底
I = sym.S.ImaginaryUnit # 虚数単位

#sympyの円周率の方を使うことをすすめる(こっちの方が量子化誤差が大きくなる.numpyも同様に大きい)
import math
pi = math.pi

# 角度変数
(J_1,J_2,J_3,J_4,J_5,J_6) = sym.symbols('J_1,J_2,J_3,J_4,J_5,J_6')

# リンクパラメータ
(a_1,a_2,a_3,d_4) = sym.symbols('a_1,a_2,a_3,d_4')

# リンクパラメータ
(j,a,d,alpha) = sym.symbols('j,a,d,alpha')

# T6
(n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z) = sym.symbols('n_x, n_y, n_z, o_x, o_y, o_z, a_x, a_y, a_z, p_x, p_y, p_z')
In [2]:
#sin.cosの簡易記述用
def S(a):
    return sym.sin(a)

def C(a):
    return sym.cos(a)
In [3]:
#回転・並進行列
def rotx(a):
    return sym.Matrix([[1, 0, 0, 0], [0, C(a), -S(a), 0], [0, S(a), C(a), 0], [0, 0, 0, 1]])

def roty(a):
    return sym.Matrix([[C(a), 0, S(a), 0], [0, 1, 0, 0], [-S(a), 0, C(a), 0], [0, 0, 0, 1]])

def rotz(a):
    return sym.Matrix([[C(a), -S(a), 0, 0], [S(a), C(a), 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]])

def trans(x, y, z):
    return sym.Matrix([[1, 0, 0, x], [0, 1, 0, y], [0, 0, 1, z], [0, 0, 0, 1]])

# DH matrix 
def DH(j, alpha, a, d):
    return rotz(j)*trans(a,0,d)*rotx(alpha)

# inverse DH matrix
def DHi(j, alpha, a, d):
    return rotx(-alpha)*trans(-a,0,-d)*rotz(-j)

DH法に基づく座標系間の関係表現

座標系 i Z_i-1軸回りに角度θ_i X_i軸周りにねじれ角α_iだけ回転 回転後のX_i-1 (=X_i)に沿って長さa_iだけ並進 Z_i-1に沿って距離d_iだけ並進
1 $J_1$ $\pi/2$ $a_1$ 0
2 $J_2+\pi/2$ 0 $a_2$ 0
3 $J_3-J_2$ $\pi/2$ $a_3$ 0
4 $J_4$ $-\pi/2$ 0 $d_4$
5 $J_5$ $\pi/2$ 0 0
6 $J_6$ 0 0 0 $

変換行列 A

In [119]:
# A1=sym.trigsimp( DH (J_1,             Pi/2, a_1, 0))
# A3=sym.trigsimp( DH (J_3 - J_2,   Pi/2, a_3, 0))
A1=sym.trigsimp( DH (J_1,             Pi/2, 0, 0))
A2=sym.trigsimp( DH (J_2+ Pi/2,       0, a_2, 0))
A3=sym.trigsimp( DH (J_3 - J_2,   Pi/2, 0, 0))
A4=sym.trigsimp( DH (J_4,            -Pi/2,      0, d_4))
A5=sym.trigsimp( DH (J_5,             Pi/2,      0, 0))
A6=sym.trigsimp( DH (J_6,                   0,     0, 0))

# inverse matrix
# A1i=sym.trigsimp( DHi (J_1,             Pi/2, a_1, 0))
# A3i=sym.trigsimp( DHi (J_3 - J_2,   Pi/2, a_3, 0))
A1i=sym.trigsimp( DHi (J_1,             Pi/2, 0, 0))
A2i=sym.trigsimp( DHi (J_2+ Pi/2,       0, a_2, 0))
A3i=sym.trigsimp( DHi (J_3 - J_2,   Pi/2, 0, 0))
A4i=sym.trigsimp( DHi (J_4,            -Pi/2,      0, d_4))
A5i=sym.trigsimp( DHi (J_5,             Pi/2,      0, 0))
A6i=sym.trigsimp( DHi (J_6,                   0,     0, 0))
In [120]:
A3
Out[120]:
$\displaystyle \left[\begin{matrix}\cos{\left(J_{2} - J_{3} \right)} & 0 & - \sin{\left(J_{2} - J_{3} \right)} & 0\\- \sin{\left(J_{2} - J_{3} \right)} & 0 & - \cos{\left(J_{2} - J_{3} \right)} & 0\\0 & 1 & 0 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$
In [121]:
# 逆行列をかけると単位行列になることの確認
ret = A2i*A2
sym.trigsimp(ret)
Out[121]:
$\displaystyle \left[\begin{matrix}1 & 0 & 0 & 0\\0 & 1 & 0 & 0\\0 & 0 & 1 & 0\\0 & 0 & 0 & 1\end{matrix}\right]$

逆運動学

image.png

image.png

In [111]:
T6=sym.Matrix([[n_x, o_x, a_x, p_x], [n_y, o_y, a_y, p_y], [n_z, o_z, a_z, p_z], [0, 0, 0, 1]])
T6
Out[111]:
$\displaystyle \left[\begin{matrix}n_{x} & o_{x} & a_{x} & p_{x}\\n_{y} & o_{y} & a_{y} & p_{y}\\n_{z} & o_{z} & a_{z} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right]$
In [122]:
# forward kinematics
A56 = sym.trigsimp( A5*A6 )
A456 = sym.trigsimp( A4*A5*A6 )
A3456 = sym.trigsimp( A3*A4*A5*A6 )
A23456 = sym.trigsimp( A2*A3*A4*A5*A6 )
T  = sym.trigsimp( A1*A2*A3*A4*A5*A6 )
In [113]:
T
Out[113]:
$\displaystyle \left[\begin{matrix}- \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} & \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} & d_{4} \cos{\left(J_{3} \right)}\\- \sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} & \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} - \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} & 0\\\left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \cos{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} & - \left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \sin{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & d_{4} \sin{\left(J_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$

順運動学計算用Cソースコードを得る

In [123]:
# Masahiro Furukawa
# Aug, 17, 2020
#
# refernce : https://qiita.com/JmpM/items/4bea4997aaf406cca3b4

# Cソースを得る
for ii in range(4):
    for jj in range(4):
        idx = jj*4+ii
        code = sym.ccode(T[idx],assign_to=('Trans['+str(jj)+']['+str(ii)+']'), standard='C89')
        print(code)
    print()
Trans[0][0] = -(sin(J_3)*cos(J_4)*cos(J_5) + sin(J_5)*cos(J_3))*cos(J_6) + sin(J_3)*sin(J_4)*sin(J_6);
Trans[1][0] = -sin(J_4)*cos(J_5)*cos(J_6) - sin(J_6)*cos(J_4);
Trans[2][0] = (-sin(J_3)*sin(J_5) + cos(J_3)*cos(J_4)*cos(J_5))*cos(J_6) - sin(J_4)*sin(J_6)*cos(J_3);
Trans[3][0] = 0;
()
Trans[0][1] = (sin(J_3)*cos(J_4)*cos(J_5) + sin(J_5)*cos(J_3))*sin(J_6) + sin(J_3)*sin(J_4)*cos(J_6);
Trans[1][1] = sin(J_4)*sin(J_6)*cos(J_5) - cos(J_4)*cos(J_6);
Trans[2][1] = -(-sin(J_3)*sin(J_5) + cos(J_3)*cos(J_4)*cos(J_5))*sin(J_6) - sin(J_4)*cos(J_3)*cos(J_6);
Trans[3][1] = 0;
()
Trans[0][2] = -sin(J_3)*sin(J_5)*cos(J_4) + cos(J_3)*cos(J_5);
Trans[1][2] = -sin(J_4)*sin(J_5);
Trans[2][2] = sin(J_3)*cos(J_5) + sin(J_5)*cos(J_3)*cos(J_4);
Trans[3][2] = 0;
()
Trans[0][3] = -a_2*sin(J_2) + d_4*cos(J_3);
Trans[1][3] = 0;
Trans[2][3] = a_2*cos(J_2) + d_4*sin(J_3);
Trans[3][3] = 1;
()

inverse kinematics

$$ % reference : https://qiita.com/namoshika/items/63db972bfd1030f8264a % 空白は表示に影響しない。コメントは"%"で始める % 下付き文字は"_a"、上付き文字は"^a" % 改行は"\\"を付ける {\boldsymbol{A}_{1}}^{-1} \boldsymbol{T}_6 = \boldsymbol{A}_2 \boldsymbol{A}_3 \boldsymbol{A}_4 \boldsymbol{A}_5 \boldsymbol{A}_6    (3.75)\\ {\boldsymbol{A}_{1}}^{-1} \boldsymbol{T}_6 = ^{1}\boldsymbol{T}_6    (3.76) % 複数文字を1要素とする際は{...}で囲う % 空白は"\quad" $$
In [124]:
T16 = sym.trigsimp(                         A1i*T6 ) #  eq(3.70)
T26 = sym.trigsimp(                   A2i*A1i*T6 ) #  eq(3.71)
T36 = sym.trigsimp(             A3i*A2i*A1i*T6 ) #  eq(3.72)
T46 = sym.trigsimp(       A4i*A3i*A2i*A1i*T6 ) #  eq(3.73)
T56 = sym.trigsimp( A5i*A4i*A3i*A2i*A1i*T6 ) #  eq(3.74)
In [125]:
# Left hand  of (3.76)
A1iT6 = T16
A1iT6
Out[125]:
$\displaystyle \left[\begin{matrix}n_{x} & o_{x} & a_{x} & p_{x}\\n_{z} & o_{z} & a_{z} & p_{z}\\- n_{y} & - o_{y} & - a_{y} & - p_{y}\\0 & 0 & 0 & 1\end{matrix}\right]$
In [117]:
# Right hand of (3.76)
A23456
Out[117]:
$\displaystyle \left[\begin{matrix}- \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} & \left(\sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{6} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{5} \right)} & d_{4} \cos{\left(J_{3} \right)}\\\left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \cos{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{3} \right)} & - \left(- \sin{\left(J_{3} \right)} \sin{\left(J_{5} \right)} + \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)}\right) \sin{\left(J_{6} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{3} \right)} \cos{\left(J_{5} \right)} + \sin{\left(J_{5} \right)} \cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & d_{4} \sin{\left(J_{3} \right)}\\\sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} & - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} & 0\\0 & 0 & 0 & 1\end{matrix}\right]$
In [126]:
# eq2 (3.76)
sym.Eq(A1iT6[11] , A23456[11])
Out[126]:
$\displaystyle - p_{y} = 0$
$$ \displaystyle \frac{ \sin{\left(J_{1} \right)} }{ \cos{\left(J_{1} \right)} } = \frac{p_{y}}{p_{x}} \\ \rightarrow \displaystyle \tan{\left({ J_{1} }\right)} = \frac{p_{y}}{p_{x}} \\ $$

Therefore $$\rightarrow \displaystyle J_{1} = atan2{\left({p_{y}},{p_{x}}\right)} \\ $$

In [77]:
# eq2 (3.76)
sym.simplify ( sym.expand( sym.Eq(
    A1iT6[3]*A1iT6[3] + A1iT6[7]*A1iT6[7],
    A23456[3]* A23456[3] + A23456[7]*A23456[7]) ) )
Out[77]:
$\displaystyle a_{2}^{2} + 2 a_{2} a_{3} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} d_{4} \sin{\left(J_{2} - J_{3} \right)} + a_{3}^{2} + d_{4}^{2} = a_{1}^{2} - 2 a_{1} p_{x} \cos{\left(J_{1} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} + p_{x}^{2} \cos^{2}{\left(J_{1} \right)} + p_{x} p_{y} \sin{\left(2 J_{1} \right)} + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} + p_{z}^{2}$
In [71]:
# eq2 (3.76)
sym.simplify ( sym.expand( sym.Eq(A1iT6[7]*A1iT6[7] , A23456[7]*A23456[7]) ) )
Out[71]:
$\displaystyle p_{z}^{2} = a_{2}^{2} \cos^{2}{\left(J_{2} \right)} + 2 a_{2} a_{3} \cos{\left(J_{2} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} d_{4} \sin{\left(J_{3} \right)} \cos{\left(J_{2} \right)} + a_{3}^{2} \cos^{2}{\left(J_{3} \right)} + a_{3} d_{4} \sin{\left(2 J_{3} \right)} + d_{4}^{2} \sin^{2}{\left(J_{3} \right)}$
In [88]:
T36
Out[88]:
$\displaystyle \left[\begin{matrix}- n_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - n_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + n_{z} \cos{\left(J_{3} \right)} & - o_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - o_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + o_{z} \cos{\left(J_{3} \right)} & - a_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - a_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + a_{z} \cos{\left(J_{3} \right)} & a_{1} \sin{\left(J_{3} \right)} - a_{2} \cos{\left(J_{2} - J_{3} \right)} - a_{3} - p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} + p_{z} \cos{\left(J_{3} \right)}\\n_{x} \sin{\left(J_{1} \right)} - n_{y} \cos{\left(J_{1} \right)} & o_{x} \sin{\left(J_{1} \right)} - o_{y} \cos{\left(J_{1} \right)} & a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)} & p_{x} \sin{\left(J_{1} \right)} - p_{y} \cos{\left(J_{1} \right)}\\n_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + n_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + n_{z} \sin{\left(J_{3} \right)} & o_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + o_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + o_{z} \sin{\left(J_{3} \right)} & a_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + a_{z} \sin{\left(J_{3} \right)} & - a_{1} \cos{\left(J_{3} \right)} + a_{2} \sin{\left(J_{2} - J_{3} \right)} + p_{x} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + p_{z} \sin{\left(J_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$
In [91]:
A456
Out[91]:
$\displaystyle \left[\begin{matrix}- \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} & - \sin{\left(J_{4} \right)} \cos{\left(J_{6} \right)} - \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} \cos{\left(J_{5} \right)} & \sin{\left(J_{5} \right)} \cos{\left(J_{4} \right)} & 0\\\sin{\left(J_{4} \right)} \cos{\left(J_{5} \right)} \cos{\left(J_{6} \right)} + \sin{\left(J_{6} \right)} \cos{\left(J_{4} \right)} & - \sin{\left(J_{4} \right)} \sin{\left(J_{6} \right)} \cos{\left(J_{5} \right)} + \cos{\left(J_{4} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} & 0\\- \sin{\left(J_{5} \right)} \cos{\left(J_{6} \right)} & \sin{\left(J_{5} \right)} \sin{\left(J_{6} \right)} & \cos{\left(J_{5} \right)} & d_{4}\\0 & 0 & 0 & 1\end{matrix}\right]$
In [104]:
# eq2 (3.76)
for idx in [3,11]:
    display(sym.simplify ( sym.expand( sym.Eq( T36[idx]*T36[idx], A456[idx]* A456[idx])) ) )
$\displaystyle a_{1}^{2} \sin^{2}{\left(J_{3} \right)} - 2 a_{1} a_{2} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{1} a_{3} \sin{\left(J_{3} \right)} - 2 a_{1} p_{x} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} + a_{1} p_{z} \sin{\left(2 J_{3} \right)} + a_{2}^{2} \cos^{2}{\left(J_{2} - J_{3} \right)} + 2 a_{2} a_{3} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{z} \cos{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} + a_{3}^{2} + 2 a_{3} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} + 2 a_{3} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} - 2 a_{3} p_{z} \cos{\left(J_{3} \right)} + p_{x}^{2} \sin^{2}{\left(J_{3} \right)} \cos^{2}{\left(J_{1} \right)} + 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - \frac{p_{x} p_{z} \left(- \sin{\left(J_{1} - 2 J_{3} \right)} + \sin{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} - \frac{p_{y} p_{z} \left(\cos{\left(J_{1} - 2 J_{3} \right)} - \cos{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{z}^{2} \cos^{2}{\left(J_{3} \right)} = 0$
$\displaystyle d_{4}^{2} = a_{1}^{2} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} a_{2} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} - 2 a_{1} p_{x} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - a_{1} p_{z} \sin{\left(2 J_{3} \right)} + a_{2}^{2} \sin^{2}{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{x} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} p_{z} \sin{\left(J_{3} \right)} \sin{\left(J_{2} - J_{3} \right)} + p_{x}^{2} \cos^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + \frac{p_{x} p_{z} \left(- \sin{\left(J_{1} - 2 J_{3} \right)} + \sin{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + \frac{p_{y} p_{z} \left(\cos{\left(J_{1} - 2 J_{3} \right)} - \cos{\left(J_{1} + 2 J_{3} \right)}\right)}{2} + p_{z}^{2} \sin^{2}{\left(J_{3} \right)}$
In [106]:
# eq2 (3.76)
display(sym.trigsimp ( sym.expand( sym.Eq( T36[3]*T36[3] - T36[11]*T36[11], A456[3]* A456[3]-A456[11]* A456[11])) ) )
$\displaystyle a_{1}^{2} \sin^{2}{\left(J_{3} \right)} - a_{1}^{2} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} a_{2} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{1} a_{2} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} - 2 a_{1} a_{3} \sin{\left(J_{3} \right)} - 2 a_{1} p_{x} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} + 2 a_{1} p_{x} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} + 2 a_{1} p_{y} \sin{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + 2 a_{1} p_{z} \sin{\left(2 J_{3} \right)} - a_{2}^{2} \sin^{2}{\left(J_{2} - J_{3} \right)} + a_{2}^{2} \cos^{2}{\left(J_{2} - J_{3} \right)} + 2 a_{2} a_{3} \cos{\left(J_{2} - J_{3} \right)} + 2 a_{2} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{x} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{3} \right)} + 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{2} - J_{3} \right)} \cos{\left(J_{3} \right)} - 2 a_{2} p_{z} \sin{\left(J_{3} \right)} \sin{\left(J_{2} - J_{3} \right)} - 2 a_{2} p_{z} \cos{\left(J_{3} \right)} \cos{\left(J_{2} - J_{3} \right)} + a_{3}^{2} + 2 a_{3} p_{x} \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} + 2 a_{3} p_{y} \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} - 2 a_{3} p_{z} \cos{\left(J_{3} \right)} + p_{x}^{2} \sin^{2}{\left(J_{3} \right)} \cos^{2}{\left(J_{1} \right)} - p_{x}^{2} \cos^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} + 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} \cos{\left(J_{1} \right)} - 2 p_{x} p_{y} \sin{\left(J_{1} \right)} \cos{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - p_{x} p_{z} \left(- \sin{\left(J_{1} - 2 J_{3} \right)} + \sin{\left(J_{1} + 2 J_{3} \right)}\right) + p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \sin^{2}{\left(J_{3} \right)} - p_{y}^{2} \sin^{2}{\left(J_{1} \right)} \cos^{2}{\left(J_{3} \right)} - p_{y} p_{z} \left(\cos{\left(J_{1} - 2 J_{3} \right)} - \cos{\left(J_{1} + 2 J_{3} \right)}\right) - p_{z}^{2} \sin^{2}{\left(J_{3} \right)} + p_{z}^{2} \cos^{2}{\left(J_{3} \right)} = - d_{4}^{2}$
In [92]:
# eq2 (3.76)
idx=6
sym.Eq(A456[idx] , T36[idx])
Out[92]:
$\displaystyle \sin{\left(J_{4} \right)} \sin{\left(J_{5} \right)} = a_{x} \sin{\left(J_{1} \right)} - a_{y} \cos{\left(J_{1} \right)}$
In [86]:
sym.trigsimp(             A1*A2*A3*A4)
Out[86]:
$\displaystyle \left[\begin{matrix}\sin{\left(J_{1} \right)} \sin{\left(J_{4} \right)} - \sin{\left(J_{3} \right)} \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} & - \frac{\cos{\left(J_{1} - J_{3} \right)}}{2} - \frac{\cos{\left(J_{1} + J_{3} \right)}}{2} & \sin{\left(J_{1} \right)} \cos{\left(J_{4} \right)} + \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} & \left(a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\right) \cos{\left(J_{1} \right)}\\- \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \cos{\left(J_{4} \right)} - \sin{\left(J_{4} \right)} \cos{\left(J_{1} \right)} & - \frac{\sin{\left(J_{1} - J_{3} \right)}}{2} - \frac{\sin{\left(J_{1} + J_{3} \right)}}{2} & \sin{\left(J_{1} \right)} \sin{\left(J_{3} \right)} \sin{\left(J_{4} \right)} - \cos{\left(J_{1} \right)} \cos{\left(J_{4} \right)} & \left(a_{1} - a_{2} \sin{\left(J_{2} \right)} - a_{3} \sin{\left(J_{3} \right)} + d_{4} \cos{\left(J_{3} \right)}\right) \sin{\left(J_{1} \right)}\\\cos{\left(J_{3} \right)} \cos{\left(J_{4} \right)} & - \sin{\left(J_{3} \right)} & - \sin{\left(J_{4} \right)} \cos{\left(J_{3} \right)} & a_{2} \cos{\left(J_{2} \right)} + a_{3} \cos{\left(J_{3} \right)} + d_{4} \sin{\left(J_{3} \right)}\\0 & 0 & 0 & 1\end{matrix}\right]$
In [85]:
sym.trigsimp(            A5i*A6i*T6)
Out[85]:
$\displaystyle \left[\begin{matrix}a_{x} \sin{\left(J_{5} \right)} + \left(n_{x} \cos{\left(J_{6} \right)} - o_{x} \sin{\left(J_{6} \right)}\right) \cos{\left(J_{5} \right)} & - a_{x} \cos{\left(J_{5} \right)} + \left(n_{x} \cos{\left(J_{6} \right)} - o_{x} \sin{\left(J_{6} \right)}\right) \sin{\left(J_{5} \right)} & n_{x} \sin{\left(J_{6} \right)} + o_{x} \cos{\left(J_{6} \right)} & p_{x}\\a_{y} \sin{\left(J_{5} \right)} + \left(n_{y} \cos{\left(J_{6} \right)} - o_{y} \sin{\left(J_{6} \right)}\right) \cos{\left(J_{5} \right)} & - a_{y} \cos{\left(J_{5} \right)} + \left(n_{y} \cos{\left(J_{6} \right)} - o_{y} \sin{\left(J_{6} \right)}\right) \sin{\left(J_{5} \right)} & n_{y} \sin{\left(J_{6} \right)} + o_{y} \cos{\left(J_{6} \right)} & p_{y}\\a_{z} \sin{\left(J_{5} \right)} + \left(n_{z} \cos{\left(J_{6} \right)} - o_{z} \sin{\left(J_{6} \right)}\right) \cos{\left(J_{5} \right)} & - a_{z} \cos{\left(J_{5} \right)} + \left(n_{z} \cos{\left(J_{6} \right)} - o_{z} \sin{\left(J_{6} \right)}\right) \sin{\left(J_{5} \right)} & n_{z} \sin{\left(J_{6} \right)} + o_{z} \cos{\left(J_{6} \right)} & p_{z}\\0 & 0 & 0 & 1\end{matrix}\right]$